//
//  Matrix4x4.cpp
//  TrackingSim
//
//  Created by Garrett Manka on 12/31/11.
//  Copyright (c) 2011. All rights reserved.
//

#include <math/Matrix4x4.h>

Matrix4x4 operator*(const Matrix4x4& a, const Matrix4x4& b)
{
    Matrix4x4 c;
    
    for( int i = 0 ; i < 4 ; ++i )
    {     
        for(int col = 0 ; col < 4 ; ++col )
        {
            double inner_product = 0;
            
            for( int j = 0 ; j < 4 ; ++j )
            {
                inner_product += a.m[i][j] * b.m[j][col];
            }
            
            c.m[i][col] = inner_product;
        }
    }
    
    return c;
}

double& Matrix4x4::operator()(int i, int j)
{
    return m[i][j];
}

const double* Matrix4x4::mat_trans()
{
	for(int i = 0 ; i < 4 ; ++i)
	{
		for( int j = 0 ; j < 4 ; ++j )
		{
			m_trans[i][j] = m[j][i];
		}
	}

	return &m_trans[0][0];
}
